#include "AStarSearch.h"


AStarSearch::AStarSearch(void)
{
}


AStarSearch::~AStarSearch(void)
{
}

const char* AStarSearch::getFileName() {
	return "AStartSearch.gpi";
}

node* AStarSearch::search(Graph* g, Vector2 start, Vector2 goal, bool penalized) {
	node* goalNode = g->getNode(goal);

	goalNode = performLimitedSearchOptimized(g->getNode(start), goalNode, NULL, penalized);

	return goalNode;
}

void AStarSearch::performSearch(node* start, node* goal, Grapher* gr, bool penalized, int maxExpandVectors) {
	queue.push(queue_node::newQN(0, start));
	node* current = NULL;
	node* next = NULL;
	edge* e = NULL;
	queue_node qn;
	goal->isGoal = true;
	int secondCount = 0;
	float d = 0;
	double weight = 0;
	Vector2 vGoal(goal->x, goal->y);

	float cur = 0;

	while(!queue.empty()){
		qn = queue.top();

		current = qn.graph_node;
		
		queue.pop();

		if(current->isGoal) 
			break;
		if(current->flag == 1)	
			continue;

		current->flag = 1;

		if(cur > qn.cost)
			

		cur = qn.cost;

		if(gr != NULL && current->previous != NULL){
			gr->addVector(Vector2(current->previous->x, current->previous->y), Vector2(current->x, current->y), 3);
		}
		
		for(int i = current->neighbors.size()-1; i >= 0; i --){
			e = &current->neighbors[i];
			next = e->to;
			
			if(next->flag == 1 || next->occupied) continue;
			
			if(penalized){
				if(current->nextToOccupied && next->nextToOccupied)
					weight = e->weight*PENTALTY_1;
				else if(current->nextToOccupied)
					weight = e->weight*PENTALTY_2;
				else if(next->nextToOccupied)
					weight = e->weight*PENTALTY_3;
				else
					weight = e->weight;
			}
			else
				weight = e->weight;
			
			d = dist(Vector2(next->x, next->y), vGoal);
			if(next->cost > 0 && next->cost <= qn.pathCost + weight + d) continue;
			
			next->previous = current;
			next->cost = qn.pathCost + weight + d;
			queue.push(queue_node::newQN(next->cost, qn.pathCost + weight,  next));
		}

		secondCount ++;
		if(maxExpandVectors > 0 && secondCount >= maxExpandVectors) break;
		if(secondCount % 1000 == 0)
			std::cout  << "Expanded " << secondCount << " nodes\n";
	}

	goal->isGoal = false;
}

void AStarSearch::performSearchOptimized(node* start, node* goal, Grapher* gr, bool penalized) {
	queue.push(queue_node::newQN(0, start));
	node* current = NULL;
	node* next = NULL;
	edge* e = NULL;
	queue_node qn;
	goal->isGoal = true;
	float d = 0;
	double weight = 0;
	Vector2 vGoal(goal->x, goal->y);

	while(!queue.empty()){
		qn = queue.top();
		current = qn.graph_node;
		queue.pop();

		if(current->isGoal) break;
		if(current->flag == 1) continue;

		current->flag = 1;

		for(int i = current->neighbors.size()-1; i >= 0; i --){
			e = &current->neighbors[i];
			next = e->to;
			
			if(next->flag == 1) continue;
			
			if(penalized){
				if(current->nextToOccupied && next->nextToOccupied)
					weight = e->weight*PENTALTY_1;
				else if(current->nextToOccupied)
					weight = e->weight*PENTALTY_2;
				else if(next->nextToOccupied)
					weight = e->weight*PENTALTY_3;
				else
					weight = e->weight;
			}
			else
				weight = e->weight;
			
			d = dist(Vector2(next->x, next->y), vGoal);
			if(next->cost > 0 && next->cost <= qn.pathCost + weight + d) continue;
			
			next->previous = current;
			next->cost = qn.pathCost + weight + d;
			queue.push(queue_node::newQN(next->cost, qn.pathCost + weight,  next));
		}
	}

	goal->isGoal = false;
}

node* AStarSearch::performLimitedSearchOptimized(node* start, node* goal, Grapher* gr, bool penalized) {
	queue.push(queue_node::newQN(0, start));
	node* current = NULL;
	node* next = NULL;
	edge* e = NULL;
	queue_node qn;
	goal->isGoal = true;
	float d = 0;
	double weight = 0;
	Vector2 vGoal(goal->x, goal->y);

	while(!queue.empty()){
		qn = queue.top();
		current = qn.graph_node;
		queue.pop();

		if(current->isGoal || !current->explored) break;
		if(current->flag == 1) continue;

		current->flag = 1;

		for(int i = current->neighbors.size()-1; i >= 0; i --){
			e = &current->neighbors[i];
			next = e->to;
			
			if(next->flag == 1 || next->occupied) continue;
			
			if(penalized){
				if(current->nextToOccupied && next->nextToOccupied)
					weight = e->weight*PENTALTY_1;
				else if(current->nextToOccupied)
					weight = e->weight*PENTALTY_2;
				else if(next->nextToOccupied)
					weight = e->weight*PENTALTY_3;
				else
					weight = e->weight;
			}
			else
				weight = e->weight;
			
			d = dist(Vector2(next->x, next->y), vGoal);
			if(next->cost > 0 && next->cost <= qn.pathCost + weight + d) continue;
			
			next->previous = current;
			next->cost = qn.pathCost + weight + d;
			queue.push(queue_node::newQN(next->cost, qn.pathCost + weight,  next));
		}
	}
	
	if(queue.empty() && !current->isGoal && current->explored)
		return NULL;

	goal->isGoal = false;

	return current;
}

float AStarSearch::dist(Vector2 v1, Vector2 v2){
	float xDiff = fabs(v1.x-v2.x), yDiff = fabs(v1.y-v2.y);
	return sqrt(pow(min(xDiff, yDiff), 2) * 2.0) + fabs(xDiff-yDiff) ;
	//return std::sqrt(std::pow(v1.x-v2.x,2)+std::pow(v1.y+v2.y,2));
}